/*
 * @Author: Mcfly
 * @Date: 2021-07-25 15:55:42
 * @LastEditTime: 2021-11-04 17:13:10
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\Ctrl\HeightCtrl.c
 * 本人github:https://github.com/McflyWZX
 */

#include "HeightCtrl.h"
#include "ti_system.h"
#include "PID.h"
#include "stdlib.h"
#include "Height.h"
#include "math.h"

HeightCtrl *HeightCtrl_Init(void)
{
    HeightCtrl *heightCtrl = (HeightCtrl *)malloc(sizeof(HeightCtrl));
    heightCtrl->PID_H = *initPID(1.8f, 0.6f, 0.116f, 1.0f, 1 / 200.0f);
    heightCtrl->PID_Speed = *initPID(1059, 550, 300, 1.2f, 1 / 200.0f);
    heightCtrl->lastHeight = 0;
    heightCtrl->count = 0;
    return heightCtrl;
}

float ErrHeight_Trans(float expectHeight, Height *nowHeight)
{
    if (fabs(expectHeight - nowHeight->h) > 0.5f)
    {
        return (expectHeight - nowHeight->h) * 0.3f;
    }
    else if (fabs(expectHeight - nowHeight->h) > 0.1f)
    {
        return (expectHeight - nowHeight->h) * 0.6f;
    }
    else return expectHeight - nowHeight->h;
}

void HeightCtrl_UpdateFrame(HeightCtrl *heightCtrl, Height *nowHeight, float expectHeight, float throttle)
{
	float expectSpeed = PIDProcess(&heightCtrl->PID_H, expectHeight - nowHeight->h);
    //float expectSpeed = PIDProcess(&heightCtrl->PID_H, expectHeight - nowHeight->h);
	float nowSpeed = nowHeight->speed;
	heightCtrl->throttle = throttle + LIMIT(-400, 600, PIDProcess(&heightCtrl->PID_Speed, expectSpeed - nowSpeed));
	//heightCtrl->lastHeight = nowHeight;
}

void HeightCtrl_Clear(HeightCtrl *heightCtrl)
{
    heightCtrl->PID_H.integral = 0;
	heightCtrl->PID_Speed.integral = 0;
}
